using System;
using System.Collections.Generic;
using System.Text;
using LocalizationTest.Tools;

namespace LocalizationTest
{
  public class MotionModelPose2D : MotionModel
  {
    public override bool StatesAreNearBy(WorldState oldState, WorldState newState)
    {
      double movedDistanceX = (newState as WorldState2D).Vector.x - (oldState as WorldState2D).Vector.x;
      double movedDistanceY = (newState as WorldState2D).Vector.y - (oldState as WorldState2D).Vector.y;

      int movedStepsX = (int)(movedDistanceX / Globals.motionRasterWidth);
      int movedStepsY = (int)(movedDistanceY / Globals.motionRasterWidth);

      if (Math.Abs(movedStepsX) > 2 || Math.Abs(movedStepsY) > 2) return false;
      else return true;
    }
    //2DO!!!
    public override double GetStateTransitionProbability(WorldState oldState, WorldState newState, ActionDescription action)
    {
      if (action is ContinuousActionPose2D)
        return GetStateTransitionProbabilityForContinuousAction(oldState, newState, action as ContinuousActionPose2D);
      else return 0;
    }

    private static double GetStateTransitionProbabilityForContinuousAction(WorldState oldState, WorldState newState, ContinuousActionPose2D action)
    {
      //ATTENTION!!! CHANGES IN THIS METHOD MIGHT REQUIRE CHANGES IN METHOD DoAction !!!
//      Vector2 oldPosition = new Vector2((oldState as WorldStatePose2D).Pose.translation.x, (oldState as WorldStatePose2D).Pose.translation.y);
//      Vector2 newPosition = new Vector2((newState as WorldStatePose2D).Pose.translation.x, (newState as WorldStatePose2D).Pose.translation.y);
      Vector2 oldPosition = (oldState as WorldStatePose2D).Pose.translation;
      Vector2 newPosition = (newState as WorldStatePose2D).Pose.translation;

      if (Globals.selfLocatorProperties.MotionNoise == NoiseType.gaussianNoSystematic)
      {
        return
          Globals.Gaussian((oldPosition - newPosition).Abs() - action.action.translation.Abs(), 5, 0);
      }
      else if (Globals.selfLocatorProperties.MotionNoise == NoiseType.noNoise)
      {
        if ((oldPosition - newPosition).Abs() - action.action.translation.Abs() == 0)
          return 1;
        else return 0;
      }
      else return 0;
    }
  }
}
